
#include "matrix.h"
#include "vector.h"
#include "transform.h"

#include <math.h>
#include <stdio.h>

int main(){

  /* This is having SERIOUS trouble (conceptually). */

  printf("Angle testing:\n");

  double x_theta;
  double y_theta;
  vm_vector x;
  vm_vector y;
  X_AXIS(x);
  Z_AXIS(x);

  Y_AXIS(y);
  X_AXIS(y);

  printf("Angle between X and Y axis: %f\n", vm_angle(x, y));

  vm_matrix *x_rot = vm_make_x_rot_mtx(x_theta);
  printf("X Rotation matrix:\n");
  vm_print_matrix(x_rot, "  ");

  vm_matrix *y_rot = vm_make_y_rot_mtx(y_theta);
  printf("Y Rotation matrix:\n");
  vm_print_matrix(y_rot, "  ");

  vm_matrix *rot = vm_matrix_mult(x_rot, y_rot);
  rot = y_rot;
  printf("Rotation matrix:\n");
  vm_print_matrix(rot, "  ");

  FILE *vect_file = fopen("../data/rot_vect.vec", "r");
  
  vm_vector v;
  while (vm_read(vect_file, &v)){

    x_theta = vm_angle(x, v);
    y_theta = vm_angle(y, v);

    printf("X rotation angle: %f\n", x_theta);
    printf("Y rotation angle: %f\n", y_theta);

    vm_matrix *x_rot = vm_make_x_rot_mtx(x_theta);
    printf("X Rotation matrix:\n");
    vm_print_matrix(x_rot, "  ");

    vm_matrix *y_rot = vm_make_y_rot_mtx(y_theta);
    printf("Y Rotation matrix:\n");
    vm_print_matrix(y_rot, "  ");

    vm_matrix *rot = vm_matrix_mult(x_rot, y_rot);
    rot = y_rot;
    printf("Rotation matrix:\n");
    vm_print_matrix(rot, "  ");

    vm_matrix *m = vm_vector_to_matrix(&v);
    vm_matrix *rotated = vm_matrix_mult(m, rot);
    vm_vector back;
    vm_matrix_to_vector(rotated, &back);
    vm_println(back);
  }

}
